/*
 *  Output.cpp
 *  NxtFace
 *
 *  Created by Dave on 10/3/06.
 *  Copyright 2006 __MyCompanyName__. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 */

#include "SBJ/NXT/Output.h"
#include "SBJ/NXT/Command.h"

namespace SBJ
{

namespace NXT
{

// Constructors
Output::Output( Device& device, OutputPort port )
: m_device( device )
{
	m_snapshot.port = port;
	get();
}
	
Output::~Output( void )
{
}
	
// Accessors
OutputPort
Output::getPort( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.port;
}

SBytes1
Output::getPower( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.power;
}

bool
Output::getMotorMode( MotorMode mode, bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.mode.isFlagSet( mode );
}
 
bool
Output::getRegulation( Regulation regulation, bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.regulation.isFlagSet( regulation );
}

SBytes1
Output::getTurnRatio( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.turn_ratio;
}

bool
Output::getMotorState( MotorState state, bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.state.isFlagSet( state );
}

UBytes4
Output::getTachoLimit( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.tacho_limit;
}

bool
Output::isInMotion( bool refresh )
{
	if ( refresh ) get();
	return ( m_snapshot.power != 0 && m_snapshot.mode.isFlagSet( mode_on ) && m_snapshot.state == state_running );
}

SBytes4
Output::getTachoCount( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.tacho_count;
}

SBytes4
Output::getBlockTachoCount( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.block_tacho_count;
}

SBytes4
Output::getRotateTachoCount( bool refresh )
{
	if ( refresh ) get();
	return m_snapshot.rotation_tacho_count;
}

// Manipulators
void
Output::setPower( SBytes1 power, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.power = power;
	set( syncType );
}

void
Output::setMotorMode( MotorMode add, MotorMode remove, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.mode.setFlag( add );
	m_snapshot.mode.unsetFlag( remove );
	set( syncType );
}

void
Output::setRegulation( Regulation add, Regulation remove, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.regulation.setFlag( add );
	m_snapshot.regulation.unsetFlag( remove );
	set( syncType );
}

void
Output::setTurnRatio( SBytes1 turnRation, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.turn_ratio = turnRation;
	set( syncType );
}

void
Output::setMotorState( MotorState add, MotorState remove, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.state.setFlag( add );
	m_snapshot.state.unsetFlag( remove );
	set( syncType );
}

void
Output::setTachoLimit( UBytes4 tachoLimit, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.tacho_limit = tachoLimit;
	set( syncType );
}

void
Output::start( SBytes1 power, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.power = power;
	m_snapshot.mode.setFlag( mode_on );
	m_snapshot.mode.setFlag( mode_regulated );
	m_snapshot.mode.setFlag( mode_brake );
	m_snapshot.regulation.setFlag( regulation_speed );
	m_snapshot.state = state_running;
	set( syncType );
}

void
Output::stop( bool useBrake, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.power = 0;
	m_snapshot.mode.setFlagTo( mode_brake, useBrake );
	set( syncType );
}

void
Output::start( SBytes1 power, double duration, MotorUnit unit, bool useBrake, bool query, Synchonous syncType )
{
	if ( query ) get();
	m_snapshot.power = power;
	m_snapshot.mode.setFlag( mode_on );
	m_snapshot.mode.setFlag( mode_regulated );
	m_snapshot.mode.setFlag( mode_brake );
	m_snapshot.regulation.setFlag( regulation_speed );
	m_snapshot.state = state_running;
	switch ( unit )
	{
		case motorUnit_degrees:
			m_snapshot.tacho_limit = (int)(duration *= 1.0);
			break;
	};
	print( *this, std::cout );
	set( syncType );
}

void
Output::resetSensor( bool relative )
{
	ResetMotorPosition::Params params = { m_snapshot.port, relative };
	Command< ResetMotorPosition >( m_device, params );
}

void
Output::set( Synchonous syncType )
{
	Command< SetOutput >( m_device, m_snapshot, syncType );
}

void
Output::get( void )
{
	m_snapshot = Command< GetOutput >( m_device, GetOutput::Params( m_snapshot.port ) ).result();
}

void
print( Output& output, std::ostream& str )
{
	str << "Port: " << (char)( 'A' + output.getPort( false ) ) << std::endl;
	str << "Power: " << (int)output.getPower( false ) << std::endl;
	str << "Is On: " << (int)output.getMotorMode( SBJ::NXT::mode_on, false ) << std::endl;
	str << "Uses Brake: " << (output.getMotorMode( SBJ::NXT::mode_brake, false ) ? "Yes" : "No") << std::endl;
	str << "Is Regulated: " << (output.getMotorMode( SBJ::NXT::mode_regulated, false ) ? "Yes" : "No") << std::endl;
	str << "Regulate Speed: " << (output.getRegulation( SBJ::NXT::regulation_speed, false ) ? "Yes" : "No") << std::endl;
	str << "Synchronize: " << (output.getRegulation( SBJ::NXT::regulation_sync, false ) ? "Yes" : "No") << std::endl;
	str << "Turn Ratio: " << (int)output.getTurnRatio( false) << std::endl;
	str << "Rampup: " << (output.getMotorState( SBJ::NXT::state_rampup, false ) ? "Yes" : "No") << std::endl;
	str << "Running: " << (output.getMotorState( SBJ::NXT::state_running, false ) ? "Yes" : "No") << std::endl;
	str << "Rampdown: " << (output.getMotorState( SBJ::NXT::state_rampdown, false ) ? "Yes" : "No") << std::endl;
	str << "Tacho Limit: " << (int)output.getTachoLimit( false ) << std::endl;
	str << "In Motion: " << (output.isInMotion( false ) ? "Yes" : "No") << std::endl;
	str << "Tacho Count: " << (int)output.getTachoCount( false ) << std::endl;
	str << "Block Count: " << (int)output.getBlockTachoCount( false ) << std::endl;
	str << "Rotate Count: " << (int)output.getRotateTachoCount( false ) << std::endl;
}

}

}